#include "conditional_filter.h"
namespace perception
{
    namespace lidar_preprocess
    {
        template <typename T1, typename T2, typename T3>
        void ConditionalFilter<T1, T2, T3>::Process(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            if (data_in->points.empty())
            {
                std::cout << "no lidar points input\n";
                return;
            }

            // 2.条件滤波
            if (config.condition_filter_config_.OrAnd) // true - and的方式,即取范围内的点云
                this->ConditionalAnd(data_in, data_out, config);

            else
                this->ConditionalOr(data_in, data_out, config);
        }
        template <typename T1, typename T2, typename T3>
        void ConditionalFilter<T1, T2, T3>::ConditionalAnd(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            // 1.格式转换 将输入点云格式转换为输出点云格式
            pcl::copyPointCloud(*data_in, *cloud);
            // auto t = cloud->points[0];
            // using T2 = decltype(t); // 可以通过decltype直接得到数据类型,再通过using定义类型,接下来就能使用
            // using T2 = decltype(cloud->points[0]);
            // using T2 = T4;
            // 2.条件滤波 typename是因为多重模板函数需要加这个
            typename pcl::ConditionAnd<T2>::Ptr range_cond(new pcl::ConditionAnd<T2>());
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("y", pcl::ComparisonOps::GT, config.condition_filter_config_.lidar_config_.ymin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("y", pcl::ComparisonOps::LT, config.condition_filter_config_.lidar_config_.ymax)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("x", pcl::ComparisonOps::GT, config.condition_filter_config_.lidar_config_.xmin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("x", pcl::ComparisonOps::LT, config.condition_filter_config_.lidar_config_.xmax)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("z", pcl::ComparisonOps::GT, config.condition_filter_config_.lidar_config_.zmin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("z", pcl::ComparisonOps::LT, config.condition_filter_config_.lidar_config_.zmax)));
            // range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new pcl::FieldComparison<PointT>("intensity", pcl::ComparisonOps::GT, 150.0)));
            // 创建滤波器
            pcl::ConditionalRemoval<T2> *condrem = new (pcl::ConditionalRemoval<T2>);
            condrem->setCondition(range_cond);
            condrem->setInputCloud(cloud);
            // condrem.setKeepOrganized(is_Keep); // 是否取反,即为true时保留范围内的点云,为false时为去除范围内点云
            // condrem.setKeepOrganized(true);	//对于散乱点云，不需要执行此语句；若输入点云为有组织的点云，此语句可保持点云的原始组织结构，不会改变行列数，点数也不会减少，被过滤掉的点用 NaN 填充。
            // 执行条件滤波
            // pcl_util::PointCloud::Ptr cloud_filtered(new pcl_util::PointCloud);
            condrem->filter(*data_out);
            delete condrem;
        }
        template <typename T1, typename T2, typename T3>
        void ConditionalFilter<T1, T2, T3>::ConditionalOr(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            // 1.格式转换 将输入点云格式转换为输出点云格式
            pcl::copyPointCloud(*data_in, *cloud);
            // 2.条件滤波
            typename pcl::ConditionOr<T2>::Ptr range_cond(new pcl::ConditionOr<T2>());
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("y", pcl::ComparisonOps::LT, config.condition_filter_config_.car_config_.car_ymin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("y", pcl::ComparisonOps::GT, config.condition_filter_config_.car_config_.car_ymax)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("x", pcl::ComparisonOps::LT, config.condition_filter_config_.car_config_.car_xmin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("x", pcl::ComparisonOps::GT, config.condition_filter_config_.car_config_.car_xmax)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("z", pcl::ComparisonOps::LT, config.condition_filter_config_.car_config_.car_zmin)));
            range_cond->addComparison(typename pcl::FieldComparison<T2>::ConstPtr(new pcl::FieldComparison<T2>("z", pcl::ComparisonOps::GT, config.condition_filter_config_.car_config_.car_zmax)));
            // range_cond2->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new pcl::FieldComparison<PointT>("intensity", pcl::ComparisonOps::GT, 150.0)));
            // 创建滤波器
            // typename pcl::ConditionalRemoval<T2>::Ptr condrem;
            pcl::ConditionalRemoval<T2> *condrem = new (pcl::ConditionalRemoval<T2>);
            condrem->setCondition(range_cond);
            condrem->setInputCloud(cloud);
            // condrem.setKeepOrganized(is_Keep); // 是否取反,即为true时保留范围内的点云,为false时为去除范围内点云
            // condrem.setKeepOrganized(true);	//对于散乱点云，不需要执行此语句；若输入点云为有组织的点云，此语句可保持点云的原始组织结构，不会改变行列数，点数也不会减少，被过滤掉的点用 NaN 填充。
            // 执行条件滤波
            // pcl_util::PointCloud::Ptr cloud_filtered(new pcl_util::PointCloud);
            condrem->filter(*data_out);
            delete condrem;
        }
        INSTANTIATE_PRODUCT(INSTANTIATE_ConditionalFilter, LIDAR_PREPROCESS_TYPES)
    }
}